﻿/**
 * MOTOR 2 - RAPID FLASH DYNAMICS
 * Copyright (c) 2007-2008, Michael Baczynski
 * 
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 * 
 * * Redistributions of source code must retain the above copyright notice,
 *   this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 * * Neither the name of the polygonal nor the names of its contributors may be
 *   used to endorse or promote products derived from this software without specific
 *   prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
package de.polygonal.motor2.collision.nbody.qtree
{
	import flash.utils.Dictionary;
	
	import de.polygonal.ds.ArrayedQueue;
	import de.polygonal.motor2.Constants;
	import de.polygonal.motor2.collision.nbody.BroadPhase;
	import de.polygonal.motor2.collision.nbody.Pair;
	import de.polygonal.motor2.collision.nbody.ShapeProxy;
	import de.polygonal.motor2.collision.nbody.qtree.QNode;
	import de.polygonal.motor2.collision.nbody.qtree.QProxy;
	import de.polygonal.motor2.collision.shapes.ShapeSkeleton;
	import de.polygonal.motor2.dynamics.contact.Contact;	

	/**
	 * a quad-tree implementation based on ideas in:
	 * - Game Programming Gems, Multi-Resolution Maps for Interaction Detection
	 * - Game Programming Gems II, Direct Access Quadtree Lookup
	 */
	public class QTree extends BroadPhase
	{
		private var _nodes:Array;
		private var _offsets:Array;
		private var _depth:int;
		private var _xs:Number;
		private var _ys:Number;
		
		private var _pairs:Dictionary;
		
		private var _proxyQue:ArrayedQueue;
		private var _proxyLookup:Array;
		
		/**
		 * initializes a new quad-tree. the quad-tree is automatically scaled to span
		 * the area specified by the width and height parameters. the root node equals depth 1,
		 * so the number of tree levels is (depth + 1).
		 * 
		 * example (unscaled size):
		 * z 8, size = (1 << 8) = 256, levels = 8 + 1
		 * z 9, size = (1 << 9) = 512, levels = 9 + 1
		 * ...
		 * 
		 * @param depth  the depth of the tree. 
		 * @param width  the width of the quad-tree.
		 * @param height the height of the quad-tree.
		 */
		public function QTree(depth:int, width:int, height:int)
		{
			//TODO move to init() function
			var i:int, x:int, y:int;
			var xsize:int, ysize:int, treeSize:int, levelEdgeSize:int;
			var offset:int = -1;
			var parentOffset:int;
			var node:QNode;
			
			_depth   = depth;
			_nodes   = new Array(_depth);
			_offsets = [-1];
			
			_pairs = new Dictionary(true);
			
			treeSize = 1 << (depth - 1);
			_xs = treeSize / width;
			_ys = treeSize / height;
			
			trace("/*////////////////////////////////////////////////////////*");
			trace(" * QUADTREE STATISTICS");
			trace(" * depth = " + (depth + 1));
			trace(" * unscaled tree size = " + treeSize + "px");
			trace(" * quad node x-scale  = " + _xs.toFixed(3));
			trace(" * quad node y-scale  = " + _ys.toFixed(3));
			trace(" * root node size = " + width + "x" + height + "px");
			trace(" * leaf node size = " + (1 / _xs) + "x" + (1 / _ys) + "px");
			trace(" ////////////////////////////////////////////////////////*/");
			
			/* create nodes, lowest -> highest res */
			for (i = 0; i < _depth; i++)
			{
				levelEdgeSize = 1 << i;
				_offsets[int(i + 1)] = offset + 1;
				
				xsize = ysize = treeSize >> i;
				xsize *= width  / treeSize;
				ysize *= height / treeSize;
				
				for (y = 0; y < levelEdgeSize; y++)
				{
					for (x = 0; x < levelEdgeSize; x++)
					{
						node = new QNode();
						node.xmin = x * xsize;
						node.ymin = y * ysize;
						node.xmax = node.xmin + xsize;
						node.ymax = node.ymin + ysize;
						
						_nodes[int((offset + 1) + (y * levelEdgeSize + x))] = node;
					}
				}
				
				offset += (1 << (i << 1));
			}
			
			/* setup parent pointer, go from highest -> lowest res */
			for (i = _depth; i > 1; i--)
			{
				levelEdgeSize = 1 << (i - 1);
				offset        = _offsets[i];
				parentOffset  = _offsets[int(i - 1)];
				
				for (y = 0; y < levelEdgeSize; y++)
					for (x = 0; x < levelEdgeSize; x++)
						_nodes[int(offset + y * levelEdgeSize + x)].parent = 
						_nodes[int(parentOffset + (y >> 1) * (levelEdgeSize >> 1) + (x >> 1))];
			}
			
			var sp:QProxy;
			var k:int = Constants.k_maxProxies;
			
			_proxyLookup = new Array(k);
			_proxyQue = new ArrayedQueue(k);
			for (i = 0; i < k; i++)
			{
				sp = new QProxy();
				sp.id = i + 1;
				_proxyQue.enqueue(sp);
				_proxyLookup[sp.id] = sp;
			}
			
			proxyList = null;
			//_pairs = new Dictionary(true);
			
			pairBuffer = new Array(Constants.k_maxPairs);
			pairBufferCount = 0;
		}
		
		/**
		 * use this to see if your proxy is in range. if it is not in range,
		 * it should be destroyed because otherwise quadtree insertion would fail.
		 * 
		 * @return true if the proxy is in range, otherwise false.
		 */
		override public function insideBounds(xmin:Number, ymin:Number, xmax:Number, ymax:Number):Boolean
		{
			if (xmin <  _xmin) return false;
			if (xmax >= _xmax) return false;
			if (ymin <  _ymin) return false;
			if (ymax >= _ymax) return false;
			return true;
		}
		
		private function searchProxy(p:QProxy):void
		{
			var hash:int;
			var node:QNode, pair:Pair;
			
			var p1:QProxy, s1:ShapeSkeleton, proxyId1:int;
			var p2:QProxy, s2:ShapeSkeleton, proxyId2:int;
			
			p1 = p;
			s1 = p1.shape;
			proxyId1 = p1.id;
			
			node = p1.node;
			
			/* search contained node (brute-force) */
			p2 = p1.nextNodeProxy;
			while (p2)
			{
				s2 = p2.shape;
				proxyId2 = p2.id;
				
				if (s1.xmin > s2.xmax || s1.xmax < s2.xmin || s1.ymin > s2.ymax || s1.ymax < s2.ymin)
				{
					if (p1.overlapCount > 0 && p2.overlapCount > 0)
					{
						if (proxyId1 < proxyId2)
							hash = (proxyId1 << 16) | (proxyId2 << 8);
						else
							hash = (proxyId2 << 16) | (proxyId1 << 8);
						
						pair = _pairs[hash];
						if (pair)
						{
							p1.overlapCount--;
							p2.overlapCount--;
							
							_pairHandler.pairRemoved(proxyId1, proxyId2, pair.userData as Contact);
							_pairs[hash] = undefined;
						}
					}
				}
				else
				{
					if (proxyId1 < proxyId2)
						hash = (proxyId1 << 16) | (proxyId2 << 8);
					else
						hash = (proxyId2 << 16) | (proxyId1 << 8);
					
					if (_pairs[hash] == undefined)
					{
						p1.overlapCount++;
						p2.overlapCount++;
						_pairs[hash] = new Pair(proxyId1, proxyId2, _pairHandler.pairAdded(s1, s2));
					}
				}
				p2 = p2.nextNodeProxy;
			}
			
			/* bubble up */
			node = node.parent;
			while (node)
			{
				p2 = node.head;
				while (p2)
				{
					s2 = p2.shape;
					proxyId2 = p2.id;
					
					if (s1.xmin > s2.xmax || s1.xmax < s2.xmin || s1.ymin > s2.ymax || s1.ymax < s2.ymin)
					{
						if (p1.overlapCount > 0 && p2.overlapCount > 0)
						{
							if (proxyId1 < proxyId2)
								hash = (proxyId1 << 16) | (proxyId2 << 8);
							else
								hash = (proxyId2 << 16) | (proxyId1 << 8);
							
							pair = _pairs[hash];
							if (pair)
							{
								p1.overlapCount--;
								p2.overlapCount--;
								
								_pairHandler.pairRemoved(proxyId1, proxyId2, pair.userData as Contact);
								_pairs[hash] = undefined;
							}
						}
					}
					else
					{
						if (proxyId1 < proxyId2)
							hash = (proxyId1 << 16) | (proxyId2 << 8);
						else
							hash = (proxyId2 << 16) | (proxyId1 << 8);
						
						if (_pairs[hash] == undefined)
						{
							p1.overlapCount++;
							p2.overlapCount++;
							_pairs[hash] = new Pair(proxyId1, proxyId2, _pairHandler.pairAdded(s1, s2));
						}
					}
					p2 = p2.nextNodeProxy;
				}
				
				node = node.parent;
			}
		}
		
		/**
		 * queries the quad-tree for intersecting proxies.
		 * this creates and removes pairs which are the send
		 * to the narrow-phase.
		 */
		override public function search():void
		{
			var hash:int;
			var node:QNode, pair:Pair;
			
			var p1:QProxy, s1:ShapeSkeleton, proxyId1:int;
			var p2:QProxy, s2:ShapeSkeleton, proxyId2:int;
			
			p1 = QProxy(proxyList);
			while (p1)
			{
				s1 = p1.shape;
				proxyId1 = p1.id;
				
				node = p1.node;
				
				/* search contained node (brute-force) */
				p2 = p1.nextNodeProxy;
				while (p2)
				{
					s2 = p2.shape;
					proxyId2 = p2.id;
					
					if (s1.xmin > s2.xmax || s1.xmax < s2.xmin || s1.ymin > s2.ymax || s1.ymax < s2.ymin)
					{
						if (p1.overlapCount > 0 && p2.overlapCount > 0)
						{
							if (proxyId1 < proxyId2)
								hash = (proxyId1 << 16) | (proxyId2 << 8);
							else
								hash = (proxyId2 << 16) | (proxyId1 << 8);
							
							pair = _pairs[hash];
							if (pair)
							{
								p1.overlapCount--;
								p2.overlapCount--;
								
								_pairHandler.pairRemoved(proxyId1, proxyId2, pair.userData as Contact);
								_pairs[hash] = undefined;
							}
						}
					}
					else
					{
						if (proxyId1 < proxyId2)
							hash = (proxyId1 << 16) | (proxyId2 << 8);
						else
							hash = (proxyId2 << 16) | (proxyId1 << 8);
						
						if (_pairs[hash] == undefined)
						{
							p1.overlapCount++;
							p2.overlapCount++;
							_pairs[hash] = new Pair(proxyId1, proxyId2, _pairHandler.pairAdded(s1, s2));
						}
					}
					p2 = p2.nextNodeProxy;
				}
				
				/* bubble up */
				node = node.parent;
				while (node)
				{
					p2 = node.head;
					while (p2)
					{
						s2 = p2.shape;
						proxyId2 = p2.id;
						
						if (s1.xmin > s2.xmax || s1.xmax < s2.xmin || s1.ymin > s2.ymax || s1.ymax < s2.ymin)
						{
							if (p1.overlapCount > 0 && p2.overlapCount > 0)
							{
								if (proxyId1 < proxyId2)
									hash = (proxyId1 << 16) | (proxyId2 << 8);
								else
									hash = (proxyId2 << 16) | (proxyId1 << 8);
								
								pair = _pairs[hash];
								if (pair)
								{
									p1.overlapCount--;
									p2.overlapCount--;
									
									_pairHandler.pairRemoved(proxyId1, proxyId2, pair.userData as Contact);
									_pairs[hash] = undefined;
								}
							}
						}
						else
						{
							if (proxyId1 < proxyId2)
								hash = (proxyId1 << 16) | (proxyId2 << 8);
							else
								hash = (proxyId2 << 16) | (proxyId1 << 8);
							
							if (_pairs[hash] == undefined)
							{
								p1.overlapCount++;
								p2.overlapCount++;
								_pairs[hash] = new Pair(proxyId1, proxyId2, _pairHandler.pairAdded(s1, s2));
							}
						}
						p2 = p2.nextNodeProxy;
					}
					
					node = node.parent;
				}
				p1 = QProxy(p1.next);
			}
		}
		
		/**
		 * computes the node level for a given proxy.
		 * @param p the proxy for which query should be done.
		 * @return the proxy's node level.
		 */
		public function getNodeLevel(p:QProxy):int
		{
			var xr:int, yr:int, level:int;
			var s:ShapeSkeleton = p.shape;
			
			/* XOR together the start and end positions on each axis. */
			xr = int(s.xmin * _xs) ^ int(s.xmax * _xs);
			yr = int(s.ymin * _ys) ^ int(s.ymax * _ys);
			
			/* each bit in the result indicates that the range volume crosses
			a point at the corresponding power of 2. the bit position of the
			highest 1 bit indicates how many levels above the bottom of the
			quadtree the range can first be properly placed. */
			level = _depth;
			
			/* count highest bit position
			to get number of tree levels - bit position */
			while (xr + yr != 0)
			{
				xr >>= 1;
				yr >>= 1;
				level = ~-level;
			}
			return level;
		}
		
		/**
		 * returns the node containing the given proxy.
		 * @param p the proxy for which the query should done.
		 * @return the quad node containing the given proxy.
		 */
		public function getNodeContaining(p:QProxy):QNode
		{
			var xl:int, yt:int, xr:int, yr:int, level:int, shift:int;
			var s:ShapeSkeleton = p.shape;
			xr = (xl = s.xmin * _xs) ^ int(s.xmax * _xs);
			yr = (yt = s.ymin * _ys) ^ int(s.ymax * _ys);
			
			level = _depth;
			while (xr + yr != 0)
			{
				xr >>= 1;
				yr >>= 1;
				level = ~-level;
			}
			
			/* lookup node pointer in a 2D array stored linearly */
			/* scale coordinates for quadtree level */
			shift = _depth - level;
			xl >>= shift;
			yt >>= shift;
			
			return _nodes[int(_offsets[level] + (yt << (~-level)) + xl)];
		}
		
		/**
		 * inserts a proxy into the quad tree.
		 * @param the proxy to be inserted into the quad tree.
		 */
		public function insert(p:QProxy):void
		{
			var xl:int, yt:int, xr:int, yr:int, level:int, shift:int;
			var s:ShapeSkeleton = p.shape;
			var n:QNode;
			
			xr = (xl = s.xmin * _xs) ^ int(s.xmax * _xs);
			yr = (yt = s.ymin * _ys) ^ int(s.ymax * _ys);
			
			level = _depth;
			while (xr + yr != 0)
			{
				xr >>= 1;
				yr >>= 1;
				level = ~-level;
			}
			shift = _depth - level;
			
			p.x = xl >>= shift;
			p.y = yt >>= shift;
			
			/* append to quad node */
			n = p.node = _nodes[int(_offsets[level] + (yt << (~-level)) + xl)];
			if (n.size == 0)
				n.head = n.tail = p;
			else
			{
				p.prevNodeProxy = n.tail;
				n.tail.nextNodeProxy = p;
				n.tail = p;
			}
			p.nextNodeProxy = null;
			n.size++;
		}
		
		//TODO support resetProxy()
		override public function moveProxy(proxyId:int):void
		{
			var p:QProxy = _proxyLookup[proxyId];
			update(p);
			//searchProxy(p);
		}
		
		private function update(p:QProxy):void
		{
			var xl:int, yt:int, xr:int, yr:int, level:int, shift:int;
			var s:ShapeSkeleton = p.shape;
			
			/* compute new quad tree position */
			xr = (xl = s.xmin * _xs) ^ int(s.xmax * _xs);
			yr = (yt = s.ymin * _ys) ^ int(s.ymax * _ys);
			
			level = _depth;
			while (xr + yr != 0)
			{
				xr >>= 1;
				yr >>= 1;
				level = ~-level;
			}
			
			shift = _depth - level;
			xl >>= shift;
			yt >>= shift;
			
			/* early out */
			if (xl == p.x && yt == p.y) return;
			
			p.x = xl;
			p.y = yt;
			
			/* shift to new node */
			p.remove();
			
			var node:QNode = p.node = _nodes[int(_offsets[level] + (yt << (~-level)) + xl)];
			if (node.size == 0)
				node.head = node.tail = p;
			else
			{
				p.prevNodeProxy = node.tail;
				node.tail.nextNodeProxy = p;
				node.tail = p;
			}
			p.nextNodeProxy = null;
			node.size++;
		}
		
		override public function createProxy(groupIndex:int, categoryBits:int, maskBits:int, shape:ShapeSkeleton):int
		{
			//TODO better decorate ShapeProxy with QProxy ?
			var proxy:QProxy = _proxyQue.dequeue();
			proxy.shape          = shape;
			proxy.groupIndex     = groupIndex;
			proxy.categoryBits   = categoryBits;
			proxy.maskBits       = maskBits;
			
			_proxyLookup[proxy.id] = proxy;
			
			proxy.next = proxyList;
			if (proxyList) proxyList.prev = proxy;
			proxyList = proxy;
			
			insert(proxy);
			
			return proxy.id;
		}
		
		override public function removeProxy(proxyId:int):void
		{
			var proxy:QProxy = _proxyLookup[proxyId];
			if (proxy.prev) proxy.prev.next = proxy.next;
			if (proxy.next) proxy.next.prev = proxy.prev;
			if (proxy == proxyList) proxyList = proxy.next;
			
			proxy.prev = proxy.next = null;
			
			proxy.remove();
			
			proxy.init();
			_proxyQue.enqueue(proxy);
		}
	}
}